//pc6 = pin2; 1= pin3 ... 6=pin8; HU=pin9;HD=pin10
int keyPins[9] = {2,3,4,5,6,7,8,9,10};
int keyStates[9] = {HIGH,HIGH,HIGH,HIGH,HIGH,HIGH,HIGH,HIGH,HIGH};
long keyTimeStamps[9] = {0,0,0,0,0,0,0,0,0};

int potentialPins[4] = {A0,A1,A2,A3};

int servoValuesInit[7] = {350,242,280,284,480,290,304};
int servoValues[7] =     {350,242,280,284,480,290,304};
int servoMin[7] =        {308,112,100,108,98,98,116};
int servoMax[7] =        {457,474,476,476,530,536,500};


void setup() {
  // put your setup code here, to run once:
  for ( int i=0;i<9;i++) {
    pinMode(keyPins[i],INPUT_PULLUP );
  }
  for ( int i=0;i<4;i++ ) {
    pinMode(potentialPins[i],INPUT);
  }
  Serial.begin(115200);

  while (!Serial) {
  ; // wait for serial port to connect. Needed for Leonardo only
  }
  
  //initServo();
}

void loop() {
  
  for (int i=0;i<9;i++) {
    if ( digitalRead(keyPins[i]) == LOW) {
      if ( (millis() - keyTimeStamps[i])>100  ) {
        //Serial.println(i);
        //delay(100);
        switch (i) {
          case 0 : //Serial.println('P');
            sendCMD(-1,0);
            break;
          case 1 : //Serial.println('1');
            runServo(0, true);
            break;
          case 2 : //Serial.println('2');
            runServo(0, false);
            break;
          case 3 : //Serial.println('3');
            runServo(3, false);
            break;
          case 4 : //Serial.println('4'); 
            runServo(3, true);
            break;
          case 5 : //Serial.println('5');
            runServo(5, false);
            break;
          case 6 : //Serial.println('6');
            runServo(5, true);
            break;
          case 7 : //Serial.println('U');
            runServo(2, false);
            break;
          case 8 : //Serial.println('D');
            runServo(2, true);
            break;
        }
      }
    } else {
      keyTimeStamps[i] = millis();
    }
  }
  
  for ( int i=0;i<4;i++ ) {
    int value = analogRead(potentialPins[i]);
    
    if (value < 300) {
      //Serial.println('+');
      
      switch (i) {
        case 0 : //Serial.println('0');
          break;
        case 1 : //Serial.println('1');
          runServo(6, false);
          break;
        case 2 : //Serial.println('2');
          runServo(4, false);
          break;
        case 3 : //Serial.println('3');
          runServo(1, false);
          break;
      }
      
    } else if ( value > 700 ) {
      //Serial.println('-');
      
      switch (i) {
        case 0 : //Serial.println('0');
          break;
        case 1 : //Serial.println('1');
          runServo(6, true);
          break;
        case 2 : //Serial.println('2');
          runServo(4, true);
          break;
        case 3 : //Serial.println('3');
          runServo(1, true);
          break;
      }
    }
  }
}
//act=false :--;act=true :++;
void runServo(int num, boolean act) {
  if ( act ) {
    if ( (servoValues[num] +1) <servoMax[num] ) {
      servoValues[num] +=1;
      //pwm.setPWM(num, 0, servoValues[num] );
      sendCMD(num, servoValues[num]);
      //delay(20);
    }
  } else {
    if ( (servoValues[num] - 1) > servoMin[num] ) {
      servoValues[num] -=1;
      //pwm.setPWM(num, 0, servoValues[num] );
      sendCMD(num, servoValues[num]);
      //delay(20);
    }
  }
}
/*
void initServo() {
  for (int i=0;i<7;i++) {
    servoValues[i] = servoValuesInit[i];
    //pwm.setPWM(i, 0, servoValues[i] );
    sendCMD(i, servoValues[i]);
  }
}
*/
void sendCMD(int key,int value) {
  Serial.print((char)key);
  Serial.print((char)(value/256));
  Serial.print((char)(value%256));
  
  Serial.print((char)key);
  Serial.print((char)(value/256));
  Serial.print((char)(value%256));
  
  delay(30);
}
